#include "uni_motor.h"  //调用uni编码电机库函数
#include "uni_servo.h"  //调用uni舵机库函数
#include "WonderK210.h" //调用视觉模块库函数
#include "config.h"     //调用小车全局参数库函数

#define BORD_RATE 115200       //设定串口波特率
// #define DEBUG_CAR_FIXED_POINT  //声明小车定点位置测试变量
// #define DEBUG_CAR_CORRING_DETECT //声明小车色环检测

UNI_MOTOR motor1(1, true);     //定义编码电机1
UNI_MOTOR motor2(2, true);     //定义编码电机2
UNI_MOTOR motor3(3, true);     //定义编码电机3
UNI_MOTOR motor4(4, true);     //定义编码电机4
WonderK210* wk;//接收控制器
Find_Box_Msg_st* result;//消息存储变量
UNI_SERVO_S servo(4, 6, 7, 10);//声明舵机4,6,7,10处的舵机
int stepper_speed = 1600; //4800; //设定步进电机速度

QR_MSG qr_msg;  //声明二维码需要的变量结构体
COLOR_MSG color_msg; //声明颜色识别需要的变量结构体
COLOR_RING_MSG colorring_msg; //声明色环识别需要的变量结构体
DETECT_MSG detect_msg; //将识别到的二维码、颜色信息转换为可识别的数据
CAR_GLOBAL_PARAMETERS car_msg; //定义工训小车整体流程过程中需要用到的参数
JUDGE_CAR judge_msg;

void setup() {
  delay(4500);//开机等待500毫秒
  Serial.begin(BORD_RATE);//打开串口
  //初始化小车并赋予其初始状态===============================
  Serial.println("初始化小车并赋予其初始状态动作开始");
  my_car_init();//小车各个组件恢复到初始位置
  Serial.println("初始化小车并赋予其初始状态动作结束");
  Serial.println("程序各个组件初始化完成");
}

void loop() {
  process_manager(); //管理小车流程程序部分

  //通过色环处的线来初步校准小车的位置
  // calibrate_the_car_through_line();

  //通过色环处的圆来初步校准小车的位置
  // calibrate_the_car_through_circle();

  // single_colorring_test();
  // car_move_pwm_test();
}


